import time

import framework.Function.ModbusBasicFunction.function
from framework.Function.ModbusBasicFunction.function import *
from array import *
from utils.PublicBasicFunction.Time import *
from utils.PublicBasicFunction.PhysicalProperty import *

# 小车使能 car_num为系统小车号
def enable_car(car_num):
    enable(car_num)


# 给所有小车上使能
def enable_all_car():
    num = read_car_num()
    i = 1
    while i <= num:
        enable_car(i)
        i += 1


# 小车断使能 car_num为系统小车号
def Disable_car(car_num):
    disable_car(car_num)


# 小车位置
def car_pos(car_num):
    return read_car_position(car_num)


# 小车开始移动 car_num为系统小车号
def Car_move(car_num):
    framework.Function.ModbusBasicFunction.function.car_move(3)


# 小车目标位置 car_num为系统小车号 pos目标位置 浮点数
def car_goal_pos(car_num, pos):
    debug_car_goal_position(car_num, pos)


# 小车增量移动 car_num为系统小车号 val为移动距离 浮点数 正数为正向增量移动 负数为负向增量移动
def car_increase_move(car_num, val):
    debug_car_goal_position(car_num, val)


# 小车目标工位 car_num为系统小车号 goal_station目标工位号
def car_goal_station(car_num, goal_station):
    pos = read_process_position(goal_station)
    debug_car_goal_position(car_num, pos)


# 检查小车位置是否在某工位  car_num为系统小车号 station_num工位号
def car_pos_check_station(car_num, station_num):
    station_pos = read_station_position(station_num)
    # 等待小车停下
    address_car_speed = 40008 + ((car_num - 1) * 40)
    while read_reg_float(address_car_speed) != 0:
        sleep(0.2)
    car_pos = read_car_position(car_num)
    assert abs(car_pos - station_pos) < 0.01, " [小车位置错误:非预期的位置]"


# 检查小车位置是否符合预期 car_num为系统小车号 expect_pos 预期位置
def car_pos_check(car_num, expect_pos):
    # 等待小车停下
    address_car_speed = 40008 + ((car_num - 1) * 40)
    while read_reg_float(address_car_speed) != 0:
        sleep(0.2)
    read_pos = read_car_position(car_num)

    assert abs(read_pos - expect_pos) < 0.01, " [小车位置错误:非预期的位置]"


# 检查小车位置 用法:jog以后 jog小车的距离与前车应为安全距离 R140027 与前车的距离
def car_pos_check_safe_dis(car_num):
    # 等待小车停下
    address_car_speed = 40008 + ((car_num - 1) * 40)
    while read_reg_float(address_car_speed) != 0:
        sleep(0.2)
    dis = dis_from_front_car(car_num)
    assert 239.0 < abs(dis) < 241.0, " [小车位置错误:非预期的位置]"


# 小车运动模式调整为绝对模式 car_num为系统小车号
def car_sport_mode_abs(car_num):
    sport_mode(car_num, 0)


# 小车运动模式调整为增量模式
def car_sport_mode_increase(car_num):
    sport_mode(car_num, 1)


# 小车运动模式调整为jog+模式
def car_sport_mode_jog_plus(car_num):
    sport_mode(car_num, 2)


# 小车运动模式调整为jog-模式
def car_sport_mode_jog_reduce(car_num):
    sport_mode(car_num, 3)


# 小车运动模式调整为正向寸动模式
def car_sport_mode_inch_move_forward(car_num):
    sport_mode(car_num, 4)


# 小车运动模式调整为负向寸动模式
def car_sport_mode_inch_move_backward(car_num):
    sport_mode(car_num, 5)


# 车辆同时出发判断 car_list为数组
def car_start_together_check(car_list: list):
    time_list = []

    # 容忍度0.2秒
    tolerate = 0.2
    assert len(car_list) >= 2, "[小车数量错误,小于两个小车无法判断是否同时起步]"
    for i in car_list:
        assert car_running_status(i) == 2, "[小车状态错误非运行状态]"
        time_list.append(DateTimeUtils.get_current_timestamp())
    min_time = min(time_list)
    max_time = max(time_list)
    assert max_time - min_time < tolerate, "[错误，小车非同时出发]"


# 小车数量
def car_num():
    return read_car_num()


# 读取小车目标工位
def Read_car_goal_pos(car_num):
    return read_car_goal_pos(car_num)


# 读取小车目标工位
def Read_car_goal_station(car_num):
    return read_car_goal_station(car_num)


# 多次读取小车速度
def Read_car_speed(car_num):
    speed_list = []
    i = 0
    while i < 50:
        speed_list.append(read_car_speed(car_num))
        sleep(0.2)
        i += 1
    return speed_list


# 多次读取小车加速度
def Read_car_acc(car_num):
    acc_list = []
    i = 0
    while i < 50:
        acc_list.append(get_acc(car_num))
        i += 1
    return acc_list